
// ===============================================================================================================
// -*- C++ -*-
//
// Frustum.hpp - Frustum culling.
//
// Copyright (c) 2011 Guilherme R. Lampert
// guilherme.ronaldo.lampert@gmail.com
//
// This code is licenced under the MIT license.
//
// This software is provided "as is" without express or implied
// warranties. You may freely copy and compile this source into
// applications you distribute provided that the copyright text
// above is included in the resulting source code.
//
// ===============================================================================================================

#include <Frustum.hpp>

#if (ENABLE_SSE_INTRINSICS)

// return 1/sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
static inline __m128 _mm_inv_length_xyz(__m128 v)
{
	__m128 tmp = _mm_mul_ps(v, v);
	return (_mm_rsqrt_ps(_mm_set_ps1(tmp.m128_f32[0] + tmp.m128_f32[1] + tmp.m128_f32[2])));
}

#else

// plane *= 1/sqrt(p.a * p.a + p.b * p.b + p.c * p.c);
static inline void NormalizePlane(float p[4])
{
	const float invLen = Math::InvSqrt((p[0] * p[0]) + (p[1] * p[1]) + (p[2] * p[2]));
	p[0] *= invLen;
	p[1] *= invLen;
	p[2] *= invLen;
	p[3] *= invLen;
}

#endif // ENABLE_SSE_INTRINSICS

// =========================================================
// Frustum Class Implementation
// =========================================================

Frustum::Frustum(const Matrix4x4 & matView, const Matrix4x4 & matProj)
{
	ComputeClippingPlanes(matView, matProj);
}

void Frustum::ComputeClippingPlanes(const Matrix4x4 & matView, const Matrix4x4 & matProj)
{
	// Compute a clip matrix:

	Matrix4x4 matClip(matProj * matView);

	// Compute and normalize the 6 frustum planes:

#if (ENABLE_SSE_INTRINSICS)

	clippingPlanes[0] = _mm_sub_ps(matClip.col[3], matClip.col[0]);
	clippingPlanes[0] = _mm_mul_ps(clippingPlanes[0], _mm_inv_length_xyz(clippingPlanes[0]));

	clippingPlanes[1] = _mm_add_ps(matClip.col[3], matClip.col[0]);
	clippingPlanes[1] = _mm_mul_ps(clippingPlanes[1], _mm_inv_length_xyz(clippingPlanes[1]));

	clippingPlanes[2] = _mm_add_ps(matClip.col[3], matClip.col[1]);
	clippingPlanes[2] = _mm_mul_ps(clippingPlanes[2], _mm_inv_length_xyz(clippingPlanes[2]));

	clippingPlanes[3] = _mm_sub_ps(matClip.col[3], matClip.col[1]);
	clippingPlanes[3] = _mm_mul_ps(clippingPlanes[3], _mm_inv_length_xyz(clippingPlanes[3]));

	clippingPlanes[4] = _mm_sub_ps(matClip.col[3], matClip.col[2]);
	clippingPlanes[4] = _mm_mul_ps(clippingPlanes[4], _mm_inv_length_xyz(clippingPlanes[4]));

	clippingPlanes[5] = _mm_add_ps(matClip.col[3], matClip.col[2]);
	clippingPlanes[5] = _mm_mul_ps(clippingPlanes[5], _mm_inv_length_xyz(clippingPlanes[5]));

#else

	p[0][A] = matClip.m[ 3] - matClip.m[ 0];
	p[0][B] = matClip.m[ 7] - matClip.m[ 4];
	p[0][C] = matClip.m[11] - matClip.m[ 8];
	p[0][D] = matClip.m[15] - matClip.m[12];
	NormalizePlane(p[0]);

	p[1][A] = matClip.m[ 3] + matClip.m[ 0];
	p[1][B] = matClip.m[ 7] + matClip.m[ 4];
	p[1][C] = matClip.m[11] + matClip.m[ 8];
	p[1][D] = matClip.m[15] + matClip.m[12];
	NormalizePlane(p[1]);

	p[2][A] = matClip.m[ 3] + matClip.m[ 1];
	p[2][B] = matClip.m[ 7] + matClip.m[ 5];
	p[2][C] = matClip.m[11] + matClip.m[ 9];
	p[2][D] = matClip.m[15] + matClip.m[13];
	NormalizePlane(p[2]);

	p[3][A] = matClip.m[ 3] - matClip.m[ 1];
	p[3][B] = matClip.m[ 7] - matClip.m[ 5];
	p[3][C] = matClip.m[11] - matClip.m[ 9];
	p[3][D] = matClip.m[15] - matClip.m[13];
	NormalizePlane(p[3]);

	p[4][A] = matClip.m[ 3] - matClip.m[ 2];
	p[4][B] = matClip.m[ 7] - matClip.m[ 6];
	p[4][C] = matClip.m[11] - matClip.m[10];
	p[4][D] = matClip.m[15] - matClip.m[14];
	NormalizePlane(p[4]);

	p[5][A] = matClip.m[ 3] + matClip.m[ 2];
	p[5][B] = matClip.m[ 7] + matClip.m[ 6];
	p[5][C] = matClip.m[11] + matClip.m[10];
	p[5][D] = matClip.m[15] + matClip.m[14];
	NormalizePlane(p[5]);

#endif // ENABLE_SSE_INTRINSICS
}

bool Frustum::TestPoint(float x, float y, float z) const
{
	for (register int i = 0; i < 6; ++i)
	{
		if (p[i][A] * x + p[i][B] * y + p[i][C] * z + p[i][D] <= 0)
		{
			return (false);
		}
	}

	return (true);
}

bool Frustum::TestSphere(float x, float y, float z, float radius) const
{
	for (register int i = 0; i < 6; ++i)	
	{
		if (p[i][A] * x + p[i][B] * y + p[i][C] * z + p[i][D] <= -radius)
		{
			return (false);
		}
	}

	return (true);
}

bool Frustum::TestCube(float x, float y, float z, float size) const
{
	for (register int i = 0; i < 6; ++i)
	{
		if (p[i][A] * (x - size) + p[i][B] * (y - size) + p[i][C] * (z - size) + p[i][D] > 0)
			continue;

		if (p[i][A] * (x + size) + p[i][B] * (y - size) + p[i][C] * (z - size) + p[i][D] > 0)
			continue;

		if (p[i][A] * (x - size) + p[i][B] * (y + size) + p[i][C] * (z - size) + p[i][D] > 0)
			continue;

		if (p[i][A] * (x + size) + p[i][B] * (y + size) + p[i][C] * (z - size) + p[i][D] > 0)
			continue;

		if (p[i][A] * (x - size) + p[i][B] * (y - size) + p[i][C] * (z + size) + p[i][D] > 0)
			continue;

		if (p[i][A] * (x + size) + p[i][B] * (y - size) + p[i][C] * (z + size) + p[i][D] > 0)
			continue;

		if (p[i][A] * (x - size) + p[i][B] * (y + size) + p[i][C] * (z + size) + p[i][D] > 0)
			continue;

		if (p[i][A] * (x + size) + p[i][B] * (y + size) + p[i][C] * (z + size) + p[i][D] > 0)
			continue;

		return (false);
	}

	return (true);
}

bool Frustum::TestAABB(const Vec3 & mins, const Vec3 & maxs) const
{
	for (register int i = 0; i < 6; ++i)
	{
		if (p[i][A] * mins.x + p[i][B] * mins.y + p[i][C] * mins.z + p[i][D] > 0)
			continue;

		if (p[i][A] * maxs.x + p[i][B] * mins.y + p[i][C] * mins.z + p[i][D] > 0)
			continue;

		if (p[i][A] * mins.x + p[i][B] * maxs.y + p[i][C] * mins.z + p[i][D] > 0)
			continue;

		if (p[i][A] * maxs.x + p[i][B] * maxs.y + p[i][C] * mins.z + p[i][D] > 0)
			continue;

		if (p[i][A] * mins.x + p[i][B] * mins.y + p[i][C] * maxs.z + p[i][D] > 0)
			continue;

		if (p[i][A] * maxs.x + p[i][B] * mins.y + p[i][C] * maxs.z + p[i][D] > 0)
			continue;

		if (p[i][A] * mins.x + p[i][B] * maxs.y + p[i][C] * maxs.z + p[i][D] > 0)
			continue;

		if (p[i][A] * maxs.x + p[i][B] * maxs.y + p[i][C] * maxs.z + p[i][D] > 0)
			continue;

		return (false);
	}

	return (true);
}